from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, TextSubstitution
from launch_ros.actions import Node

def generate_launch_description():
    # 声明参数
    vehicle_id_arg = DeclareLaunchArgument(
        'vehicle_id',
        default_value="1",
        description='Vehicle ID.'
    )

    uav_namespace_arg = DeclareLaunchArgument(
        'uav_namespace',
        default_value='uav_swarms',
        description='Namespace for UAV swarms.'
    )

    vehicle_type_arg = DeclareLaunchArgument(
        'vehicle_type',
        default_value='drone',
        description='Type of vehicle.'
    )

    vehicle_num_arg = DeclareLaunchArgument(
        'vehicle_num',
        default_value="16",
        description='Number of vehicles.'
    )

    # 创建节点列表
    nodes = []

    # 创建多个节点
    for i in range(1, 16):  # 使用英文逗号
        node = Node(
            package="uav_swarms",
            executable="Follow_node",
            name=f"Follow_node_{i}",
            output="screen",
            emulate_tty=True,
            parameters=[
                {"uav_namespace": LaunchConfiguration('uav_namespace')},
                {"vehicle_id": i},  # 直接传递整数，而不是字符串
                {"vehicle_num": LaunchConfiguration('vehicle_num')},
                {"vehicle_type": LaunchConfiguration('vehicle_type')},
            ]
        )
        nodes.append(node)

    # 创建并返回 LaunchDescription
    return LaunchDescription([
        vehicle_id_arg,
        uav_namespace_arg,
        vehicle_type_arg,
        vehicle_num_arg,
        *nodes  # 解包节点列表
    ])